# coding : UTF-8
"""
作者：BingBO   时间：2022年10月23日
自动调整代码格式 ：Alt+Ctrl+L
"""
import math
import cv2 as cv
import numpy as np


# (垂直)灰度重心法
# 传入的是RGB 截取图像
# upperRowIndex ： 窗口上边界在图像中的行索引
def wds_Roi(wds, upperRowIndex=0):
    h, w, c = wds.shape
    Red = wds[:, :, 2]  # B0 G1 R2
    _, thrRed = cv.threshold(Red, 140, 255, cv.THRESH_BINARY)  # 转为二值图
    # thrRed = cv.medianBlur(thrRed, 3)
    gravityCoorList = []  # 中心点的坐标列表
    for col in range(w):
        n = 0  # 存储该列有无光点
        sum_RowIdx = 0  # 该列的中心点行索引
        for row in range(h):
            if thrRed[row, col] != 0:
                n += 1
                sum_RowIdx += row + 1
        if n:
            gravityRow = round(sum_RowIdx / n) - 1
            realRow = upperRowIndex + gravityRow  # 在原始图像中的行坐标
            gravityCoorList.append((col, realRow))  # 添加坐标（列，行）
    return gravityCoorList


# 交叉点 初略定位
def CrossPointlocation_step1(img):
    imgcopy = img.copy()  # imgcopy 用来展示窗口效果
    high, wth, chs = img.shape
    h, w = 40, 20
    rowIndexUp, rowIndexDn = 0, high - h  # 都是窗口的上边界的行索引
    # findUp, findDn = 0, 0
    wdsnum = 1
    # 1. 搜索窗口迭代
    while 1:
        wdsUp = img[rowIndexUp:rowIndexUp + h, 0: w]
        cv.rectangle(imgcopy, (0, rowIndexUp), (w - 1, rowIndexUp + h - 1), (0, 255, 0), 1)  # 在图像内部 最外圈
        Red = wdsUp[:, :, 2]  # B0 G1 R2
        _, thrRed = cv.threshold(Red, 140, 255, cv.THRESH_BINARY)  # 转为二值图
        thrRed = cv.medianBlur(thrRed, 3)
        # 判断当前窗口图像  是否存在光条
        sumX = np.sum(thrRed, axis=1)  # 1 行相加, 变成一列
        sum = np.sum(sumX, axis=0)  # 0 列相加
        if sum != 0:
            sumXIndex = np.where(sumX > 0)  # 返回（索引+ 数据类型 ）
            rowUpIdx = min(sumXIndex[0])
            findRow = rowIndexUp + rowUpIdx
            print("交叉点定位：当前窗口检测到光条，最上行索引：", findRow)
            # 定位窗口
            cv.rectangle(imgcopy, (0, findRow - 19), (19, findRow + 40), (255, 0, 255), 1)  # 在图像内部最外圈（宽20 高60）
            fitWdsUp = img[findRow - 19:findRow + 41, 0: w]
            wdsUp_row = findRow - 19
            break
        # else:
        # print("交叉点定位：当前窗口%d未检测到光条" % wdsnum)
        rowIndexUp += h
        wdsnum += 1
    wdsnum = 1
    while 1:
        wdsDn = img[rowIndexDn:rowIndexDn + h, 0: w]
        cv.rectangle(imgcopy, (0, rowIndexDn), (w - 1, rowIndexDn + h - 1), (0, 255, 0), 1)
        Red2 = wdsDn[:, :, 2]  # B0 G1 R2
        _, thrRed2 = cv.threshold(Red2, 140, 255, cv.THRESH_BINARY)  # 转为二值图
        thrRed2 = cv.medianBlur(thrRed2, 3)

        # 判断当前窗口图像  是否存在光条
        sumX = np.sum(thrRed2, axis=1)  # 1 行相加, 变成一列
        sum = np.sum(sumX, axis=0)  # 0 列相加
        if sum != 0:
            sumXIndex = np.where(sumX > 0)  # 返回（索引+ 数据类型 ）
            rowUpIdx = max(sumXIndex[0])
            findRow = rowIndexDn + rowUpIdx  # findRow：光条的最大（最下面）行索引
            print("交叉点定位：当前窗口检测到光条，最下行索引：", findRow)
            # 定位窗口
            cv.rectangle(imgcopy, (0, findRow - 40), (19, findRow + 19), (255, 0, 255), 1)  # 在图像内部最外圈（宽20 高60）
            fitWdsDn = img[findRow - 40:findRow + 20, 0: w]
            wdsDn_row = findRow - 40
            break
        # else:
        # print("交叉点定位：当前窗口%d未检测到光条" % wdsnum)
        rowIndexDn -= h
        wdsnum += 1

    # cv.imshow("fitWdsUp", fitWdsUp)
    # cv.imshow("fitWdsDn", fitWdsDn)

    # 对窗口处理，拟合出直线斜率
    gravityCoorUp = wds_Roi(fitWdsUp, wdsUp_row)  # list类型
    gravityCoorDn = wds_Roi(fitWdsDn, wdsDn_row)  # list类型

    coorarray_up = np.array(gravityCoorUp)
    coorarray_dn = np.array(gravityCoorDn)

    # 对中心点1 拟合
    pointSet1_col = coorarray_up[:, 0]  # 列
    pointSet1_row = coorarray_up[:, 1]  # 行
    poly = np.polyfit(pointSet1_col, pointSet1_row, 1)  # 用最小二乘法（含COG_V误差）拟合得到的直线参数
    fit1_k, fit1_b = poly[0], poly[1]  # A, B, C = fit_k, -1, fit_b

    p1 = gravityCoorUp[0]  # 列，行
    p2 = (400, round(fit1_k * 400 + fit1_b))
    cv.line(imgcopy, p1, p2, (255, 255, 0), 1)  # 青蓝
    start_point1 = gravityCoorUp[-1]  # 迭代窗口的最右边的中心点

    # 对中心点2 拟合
    pointSet2_col = coorarray_dn[:, 0]  # 列
    pointSet2_row = coorarray_dn[:, 1]  # 行
    poly = np.polyfit(pointSet2_col, pointSet2_row, 1)  # 用最小二乘法（含COG_V误差）拟合得到的直线参数
    fit2_k, fit2_b = poly[0], poly[1]  # A, B, C = fit_k, -1, fit_b

    p3 = gravityCoorDn[0]  # 列，行
    p4 = (400, round(fit2_k * 400 + fit2_b))
    cv.line(imgcopy, p3, p4, (255, 255, 0), 1)  # 青蓝
    start_point2 = gravityCoorDn[-1]  # 迭代窗口的最右边的中心点

    # 3. 计算交点坐标，并设置交叉点ROI
    cx = round((fit2_b - fit1_b) / (fit1_k - fit2_k))
    cy = round(fit1_k * cx + fit1_b)
    cv.circle(imgcopy, (cx, cy), 2, (255, 0, 255), -1)
    cv.rectangle(imgcopy, (cx - 80, cy - 40), (cx + 80, cy + 40), (0, 255, 0), 1)  # 列，行

    # cv.imshow("imgshow", imgcopy)
    cv.imwrite(r"F:\MVS_Data\edgeSearch.png", imgcopy)
    return cx, cy, start_point1, start_point2  # 列 行 \ (列 行)


# 对搜索小窗口内进行 交叉点定位
# img：截取的 小窗口图像
def CrossPointlocation_step2(img, lw):
    src = img.copy()
    cx, cy, start_point1, start_point2 = CrossPointlocation_step1(img)
    roih = 80
    roiw = 160
    roi_row = img[(cy - int(roih / 2)): (cy + int(roih / 2)), (cx - int(roiw / 2)):(cx + int(roiw / 2))]  # 行， 列
    cv.imshow("roi", roi_row)
    # 列，行  实际大小+1
    cv.rectangle(img, (cx - int(roiw / 2), cy - int(roih / 2)), (cx + int(roiw / 2), cy + int(roih / 2)), (0, 255, 0),
                 1)

    h, w, _ = roi_row.shape
    print((h, w))
    Red = roi_row[:, :, 2]  # B0 G1 R2
    _, threshold = cv.threshold(Red, 140, 255, cv.THRESH_BINARY)  # 转为二值图
    # threshold2 = cv.adaptiveThreshold(Red, 255, cv.ADAPTIVE_THRESH_MEAN_C, cv.THRESH_BINARY, 13, 2)
    roi = cv.medianBlur(threshold, 3)
    cv.imshow("roi_threshold", roi)
    # print(img[130, 190])

    # 直接对red通道进行计算
    sumMax = 0
    halfLw = int(lw / 2)
    for row in range(0, h):
        for col in range(0, w):
            sum = 0
            # lw: 激光条纹的垂直方向 宽度
            for rowAdd in range(0, halfLw * 3 - 1):
                for colAdd in range(0, halfLw):
                    # l1右
                    if 0 <= (row + rowAdd) < h and 0 <= (col + rowAdd + colAdd) < w:
                        sum += roi[row + rowAdd, col + rowAdd + colAdd]
                    if 0 <= (row + rowAdd) < h and 0 <= (col + rowAdd - colAdd) < w:
                        sum += roi[row + rowAdd, col + rowAdd - colAdd]
                    # l1左
                    if 0 <= (row - rowAdd) < h and 0 <= (col - rowAdd + colAdd) < w:
                        sum += roi[row - rowAdd, col - rowAdd + colAdd]
                    if 0 <= (row - rowAdd) < h and 0 <= (col - rowAdd - colAdd) < w:
                        sum += roi[row - rowAdd, col - rowAdd - colAdd]
                    # l2右
                    if 0 <= (row - rowAdd) < h and 0 <= (col + rowAdd + colAdd) < w:
                        sum += roi[row - rowAdd, col + rowAdd + colAdd]
                    if 0 <= (row - rowAdd) < h and 0 <= (col + rowAdd - colAdd) < w:
                        sum += roi[row - rowAdd, col + rowAdd - colAdd]
                    # l2左
                    if 0 <= (row + rowAdd) < h and 0 <= (col - rowAdd + colAdd) < w:
                        sum += roi[row + rowAdd, col - rowAdd + colAdd]
                    if 0 <= (row + rowAdd) < h and 0 <= (col - rowAdd - colAdd) < w:
                        sum += roi[row + rowAdd, col - rowAdd - colAdd]
            if sum >= sumMax:
                sumMax = sum
                cpRoi = (col, row)
    print("CrossPoint在roi中的精确坐标：", cpRoi)
    cv.circle(roi_row, cpRoi, 2, (255, 0, 255), -1)

    cpCol = cx - int(roiw / 2) + cpRoi[0]
    cpRow = cy - int(roih / 2) + cpRoi[1]
    CrossPoint = (cpCol, cpRow)
    print("CrossPoint在全图中的精确坐标：", CrossPoint)
    # 提取出Laser1、Laser2、以及四个区域
    regionROI, laserL1, laserL2 = regionSegment(src, CrossPoint)

    # cv.imshow("CrossPoint", roi_row)
    cv.imwrite(r"F:\MVS_Data\CrossPoint.png", roi_row)
    return CrossPoint, start_point1, start_point2, laserL1, laserL2, regionROI


def regionSegment(src, CrossPoint):
    crossRow, crossCol = CrossPoint[1], CrossPoint[0]
    LeftUpRoi = src[0:crossRow, 0:crossCol]  # Roi1
    RightUpRoi = src[0:crossRow, crossCol:]  # Roi2
    RightDownRoi = src[crossRow:, crossCol:]  # Roi3
    LeftDownRoi = src[crossRow:, 0:crossCol]  # Roi4

    # cv.imwrite(r"F:\MVS_Data\LeftUpRoi.png", LeftUpRoi)
    # cv.imwrite(r"F:\MVS_Data\LeftDownRoi.png", LeftDownRoi)
    # cv.imwrite(r"F:\MVS_Data\RightUpRoi.png", RightUpRoi)
    # cv.imwrite(r"F:\MVS_Data\RightDownRoi.png", RightDownRoi)
    regionROI = [LeftUpRoi, RightUpRoi, RightDownRoi, LeftDownRoi]  # 4个子区域列表

    laserL1 = np.zeros_like(src)
    laserL2 = np.zeros_like(src)
    laserL1[0:crossRow, 0:crossCol] = LeftUpRoi  # Roi1
    laserL1[crossRow:, crossCol:] = RightDownRoi  # Roi3
    laserL2[crossRow:, 0:crossCol] = LeftDownRoi  # Roi4
    laserL2[0:crossRow, crossCol:] = RightUpRoi  # Roi2
    # cv.imshow("laserL1", laserL1)
    cv.imwrite(r"F:\MVS_Data\laser1.png", laserL1)
    # cv.imshow("laserL2", laserL2)
    cv.imwrite(r"F:\MVS_Data\laser2.png", laserL2)
    return regionROI, laserL1, laserL2


# 边缘点定位
def edgePointlocation(srcUp, srcDn):
    print("------------边缘检测---------------")
    imgUp = srcUp.copy()  # 避免对图像进行改动
    imgDn = srcDn.copy()
    h, w = 20, 40
    h_Up, w_Up, _ = imgUp.shape
    findUp, findDn = 0, 0
    wdsnum = 1
    # 1. 搜索窗口迭代
    # 第一个窗口左边界的列索引
    edgeUpLeft = w_Up - w  # 都是窗口的左边界的列索引
    lightWidth_Up = 0
    while findUp == 0:
        wdsUp = imgUp[0: h, edgeUpLeft: edgeUpLeft + w]  # 行区间、列区间
        cv.rectangle(imgUp, (edgeUpLeft, 0), (edgeUpLeft + w - 1, h - 1), (0, 255, 0), 1)  # 在图像内部 最外圈
        Red = wdsUp[:, :, 2]  # B0 G1 R2
        _, thrRed = cv.threshold(Red, 140, 255, cv.THRESH_BINARY)  # 转为二值图

        # 判断当前窗口图像  是否存在光条
        sumY = np.sum(thrRed, axis=0)  # 0 列相加, 变成一行
        sum = np.sum(sumY, axis=0)  # 0 行相加
        if sum != 0:
            findUp = 1
            sumYIndex = np.where(sumY > 0)  # 返回（索引+ 数据类型 ）
            lightUpRight_inRoi = max(sumYIndex[0])  # 在图像中最右边的索引
            lightUpRight_inImg = edgeUpLeft + lightUpRight_inRoi
            print("边缘点搜索：上窗口检测到光条，最右列索引：", lightUpRight_inImg)

            # 定位窗口 —— 在图像内部最外圈（宽60 高20）
            cv.rectangle(imgUp, (lightUpRight_inImg - 40, 0), (lightUpRight_inImg + 19, 19), (255, 0, 255), 1)
            cv.circle(imgUp, (lightUpRight_inImg, 0), 1, (255, 0, 255), -1)
            egdeWdsUp = srcUp[0:20, lightUpRight_inImg - 40: lightUpRight_inImg + 20]

            # 计算 定位窗口（包含完全的光条段）的 光条宽度（默认没有噪音）
            Red = egdeWdsUp[:, :, 2]  # B0 G1 R2
            _, thrRed = cv.threshold(Red, 140, 255, cv.THRESH_BINARY)  # 转为二值图
            sumY = np.sum(thrRed, axis=0)  # 0 列相加, 变成一行
            sumYIndex = np.where(sumY > 0)  # 返回（索引+ 数据类型 ）
            lightWidth_Up = max(sumYIndex[0]) - min(sumYIndex[0])
        edgeUpLeft -= w
        wdsnum += 1

    wdsnum = 1
    h_Dn, w_Dn, _ = imgDn.shape
    edgeDnLeft = w_Dn - w  # 所有窗口的最左边的列索引
    lightWidth_Dn = 0
    while findDn == 0:
        wdsDn = imgDn[h_Dn - h: h_Dn, edgeDnLeft: edgeDnLeft + w]  # 行区间、列区间
        cv.rectangle(imgDn, (edgeDnLeft, h_Dn - h), (edgeDnLeft + w - 1, h_Dn - 1), (0, 255, 0), 1)  # 在图像内部 最外圈
        Red = wdsDn[:, :, 2]  # B0 G1 R2
        _, thrRed = cv.threshold(Red, 140, 255, cv.THRESH_BINARY)  # 转为二值图

        # 判断当前窗口图像  是否存在光条
        sumY = np.sum(thrRed, axis=0)  # 0 列相加, 变成一行
        sum = np.sum(sumY, axis=0)  # 0 行相加
        if sum != 0:
            findDn = 1
            sumYIndex = np.where(sumY > 0)  # 返回（索引+ 数据类型 ）
            lightDnRight_inRoi = max(sumYIndex[0])
            lightDnRight_inImg = edgeDnLeft + lightDnRight_inRoi  # edgeDnLeft：所有边缘窗口的最左边的列索引
            print("边缘点搜索：下窗口检测到光条，光条的最右列索引：", lightDnRight_inImg)

            # 定位窗口 —— 在图像内部最外圈（宽60 高20）
            cv.rectangle(imgDn, (lightDnRight_inImg - 40, h_Dn - h), (lightDnRight_inImg + 19, h_Dn - 1), (255, 0, 255),
                         1)
            cv.circle(imgDn, (lightDnRight_inImg, h_Dn - 1), 1, (255, 0, 255), -1)
            egdeWdsDn = srcDn[h_Dn - 20:h_Dn, lightDnRight_inImg - 40: lightDnRight_inImg + 20]

            # 计算 定位窗口（包含完全的光条段）的 光条宽度（默认没有噪音）
            Red = egdeWdsDn[:, :, 2]  # B0 G1 R2
            _, thrRed = cv.threshold(Red, 140, 255, cv.THRESH_BINARY)  # 转为二值图
            sumY = np.sum(thrRed, axis=0)  # 0 列相加, 变成一行
            sumYIndex = np.where(sumY > 0)  # 返回（索引+ 数据类型 ）
            lightWidth_Dn = max(sumYIndex[0]) - min(sumYIndex[0])
        edgeDnLeft -= w
        wdsnum += 1

    # 和下面迭代一致，光条上方的左边点作为起点，设置迭代窗口
    # cv.imshow("egdeWdsUp", egdeWdsUp)  # 上图的第一个窗口

    # egdeWdsUp定位窗口的 左边全图列索引
    Up_curWdsLeft_inImg = lightUpRight_inImg - 40
    # cv.imshow("egdeWdsDn", egdeWdsDn)  # 下图的第一个窗口
    Dn_curWdsLeft_inImg = lightDnRight_inImg - 40

    print("------------------上图 间断检测 roi 迭代------------------")
    h_Up, w_Up, _ = imgUp.shape
    num = 1  # 记录迭代窗口号
    cur_IterRoi = egdeWdsUp  # 初始化 将找到的上边缘窗口初始化为首个roiCur
    Previous_wds_up_row = 0
    Previous_wds_right_col = 0
    print("lightWidth_Up:", lightWidth_Up)
    while 1:
        cur_IterRoi_Red = cur_IterRoi[:, :, 2]  # B0 G1 R2
        _, cur_IterRoi_threshold = cv.threshold(cur_IterRoi_Red, 140, 255, cv.THRESH_BINARY)  # 转为二值图
        cv.imwrite(r"F:\MVS_Data\cur_IterRoi_threshold.png", cur_IterRoi_threshold)
        # 求最后的光条列坐标
        lightList = (np.sum(cur_IterRoi_threshold, axis=0))  # 0 列相加, 变成一行
        lightList2 = (np.sum(cur_IterRoi_threshold, axis=1))  # 1 行相加, 变成一列
        # 首先判断当前窗口内  有无 光点
        if np.sum(lightList, axis=0):
            ret = np.where(lightList > 0)  # 返回（索引+ 数据类型 ）
            lightColIdx = ret[0]
            lightWidth = max(lightColIdx) - min(lightColIdx)  # 检测 窗口内 光条的宽度
            # print("1. 上迭代 当前窗口光条宽度:", lightWidth)
            if lightWidth > lightWidth_Up + 5:
                print("检测当前窗口宽度较大，判断为间断")
                cv.rectangle(imgUp, (Up_startCol_inImg - 40, (num - 1) * 20),
                             (Up_startCol_inImg + 19, num * 20 - 1),
                             (0, 0, 255), 1)  # 红色
                break
            # 检测窗口内 光条的 高度
            if lightWidth < lightWidth_Up - 5:
                print("检测当前窗口宽度较小，判断为间断")
                cv.rectangle(imgUp, (Up_startCol_inImg - 40, (num - 1) * 20),
                             (Up_startCol_inImg + 19, num * 20 - 1),
                             (0, 0, 255), 1)  # 红色
                break
            ret = np.where(lightList2 > 0)  # 返回（索引+ 数据类型 ）
            lightRowIdx = ret[0]
            # print("2. 上迭代 当前窗口光条高度:", max(lightIdx2))
            if max(lightRowIdx) < 18:
                print("检测当前窗口光条高度存在空隙，判断为间断")
                cv.rectangle(imgUp, (Up_startCol_inImg - 40, (num - 1) * 20),
                             (Up_startCol_inImg + 19, num * 20 - 1),
                             (0, 0, 255), 1)  # 红色
                break
        # 还要一种 上一个窗口刚好占满 最后的光条，下一个窗口就完全没有光条，全0
        if np.sum(lightList, axis=0) == 0:
            print("检测当前窗口不存在光条，判断为间断")
            break
        # ------------ 当前窗口 没有检测到光条间断，则更新 --------------------
        Previous_wds_up_row = (num - 1) * 20
        Previous_wds_right_col = Up_curWdsLeft_inImg + 59
        # cur_IterRoi更新为下一个迭代窗口
        Up_startCol_inImg = Up_curWdsLeft_inImg + min(lightColIdx)
        cur_IterRoi = srcUp[num * 20:(num + 1) * 20, Up_startCol_inImg - 40: Up_startCol_inImg + 20]
        cv.rectangle(imgUp, (Up_startCol_inImg - 40, num * 20), (Up_startCol_inImg + 19, (num + 1) * 20 - 1),
                     (0, 255, 0), 1)  # 绿色
        num += 1
        Up_curWdsLeft_inImg = Up_startCol_inImg - 40

    # 退出迭代，设置最后的间断窗口
    # 上一个窗口：    上行坐标 Previous_wds_up_row             最右列坐标Previous_wds_right_col
    cv.rectangle(imgUp, (Previous_wds_right_col - 119, Previous_wds_up_row),
                 (Previous_wds_right_col, Previous_wds_up_row + 39), (0, 255, 255), 1)
    finRoiUp = srcUp[Previous_wds_up_row:Previous_wds_up_row + 40, Up_curWdsLeft_inImg - 60: Up_curWdsLeft_inImg + 60]

    oriUp_row = Previous_wds_up_row  # 定位窗口的上行索引
    oriUp_col = Up_curWdsLeft_inImg - 60  # 定位窗口的 左列索引
    # cv.imshow("imgUp", imgUp)
    # cv.waitKey(0)
    # cv.destroyAllWindows()

    print("------------------下图 间断检测 roi 迭代------------------")
    h_Dn, w_Dn, _ = imgDn.shape
    num = 1  # 记录迭代窗口号
    cur_IterRoi = egdeWdsDn  # 初始化 将找到的下边缘窗口初始化为首个roiCur
    print("lightWidth_Dn:", lightWidth_Dn)
    while 1:
        cur_IterRoi_Red = cur_IterRoi[:, :, 2]  # B0 G1 R2
        _, cur_IterRoi_threshold = cv.threshold(cur_IterRoi_Red, 140, 255, cv.THRESH_BINARY)  # 转为二值图
        cv.imwrite(r"F:\MVS_Data\cur_IterRoi_threshold.png", cur_IterRoi_threshold)
        # 求最后的光条列坐标
        lightList = (np.sum(cur_IterRoi_threshold, axis=0))  # 0 列相加, 变成一行
        lightList2 = (np.sum(cur_IterRoi_threshold, axis=1))  # 1 行相加, 变成一列
        # 首先判断当前窗口内  有无 光点
        if np.sum(lightList, axis=0):
            ret = np.where(lightList > 0)  # 返回（索引+ 数据类型 ）
            lightIdx = ret[0]
            lightWidth = max(lightIdx) - min(lightIdx)  # 检测 窗口内 光条的宽度
            # print("1. 下迭代 当前窗口光条宽度:", lightWidth)
            if lightWidth > lightWidth_Dn + 5:
                print("检测当前窗口宽度较大，判断为间断")
                cv.rectangle(imgDn, (Dn_startCol_inImg - 40, h_Dn - num * 20),
                             (Dn_startCol_inImg + 19, h_Dn - num * 20 + 19), (0, 0, 255), 1)  # 红色
                break
            # 检测窗口内 光条的 高度
            if lightWidth < lightWidth_Dn - 5:
                print("检测当前窗口宽度较小，判断为间断")
                cv.rectangle(imgDn, (Dn_startCol_inImg - 40, h_Dn - num * 20),
                             (Dn_startCol_inImg + 19, h_Dn - num * 20 + 19), (0, 0, 255), 1)  # 红色
                break
            ret = np.where(lightList2 > 0)  # 返回（索引+ 数据类型 ）
            lightIdx2 = ret[0]
            # print("2. 下迭代 当前窗口光条高度:", max(lightIdx2))
            if max(lightIdx2) < 18:
                print("检测当前窗口光条高度存在空隙，判断为间断")
                cv.rectangle(imgDn, (Dn_startCol_inImg - 40, h_Dn - num * 20),
                             (Dn_startCol_inImg + 19, h_Dn - num * 20 + 19), (0, 0, 255), 1)  # 红色
                break
        # 还要一种 上一个窗口刚好占满 最后的光条，下一个窗口就完全没有光条，全0
        if np.sum(lightList, axis=0) == 0:
            print("检测当前窗口不存在光条，判断为间断")
            break
        # 当前窗口的 一些参数
        Previous_wds_dn_row = h_Dn - 1 - (num - 1) * 20
        Previous_wds_right_col = Dn_curWdsLeft_inImg + 59  # Dn_curWdsLeft_inImg  当前窗口的 左边界在全图中的列索引

        # 下一个迭代窗口
        Dn_startCol_inImg = Dn_curWdsLeft_inImg + min(lightIdx)
        cur_IterRoi = srcDn[h_Dn - (num + 1) * 20:h_Dn - num * 20, Dn_startCol_inImg - 40: Dn_startCol_inImg + 20]
        # 画出下一个roi的边界
        cv.rectangle(imgDn, (Dn_startCol_inImg - 40, h_Dn - (num + 1) * 20),
                     (Dn_startCol_inImg + 19, h_Dn - num * 20 - 1), (0, 255, 0), 1)  # 绿色

        # 更新窗口，继续迭代
        num += 1
        Dn_curWdsLeft_inImg = Dn_startCol_inImg - 40

    # 退出迭代，设置最后的间断窗口
    # 上一个窗口：    上行坐标 Previous_wds_up_row             最右列坐标Previous_wds_right_col
    cv.rectangle(imgDn, (Previous_wds_right_col - 119, Previous_wds_dn_row - 39),
                 (Previous_wds_right_col, Previous_wds_dn_row), (0, 255, 255), 1)
    finRoiDn = srcDn[Previous_wds_dn_row - 39:Previous_wds_dn_row + 1,
               Previous_wds_right_col - 119:Previous_wds_right_col + 1]

    oriDn_row = Previous_wds_dn_row - 39  # 定位窗口的上行索引
    oriDn_col = Previous_wds_right_col - 119  # 定位窗口的 左列索引
    # cv.imshow("imgDn", imgDn)
    # cv.waitKey(0)
    # cv.destroyAllWindows()

    # --------------对 上图最后窗口进行边缘点定位----------------
    finRoiUp_Red = finRoiUp[:, :, 2]  # B0 G1 R2
    _, finRoiUp = cv.threshold(finRoiUp_Red, 140, 255, cv.THRESH_BINARY)  # 转为二值图
    # 求最后的光条列坐标
    lightList = (np.sum(finRoiUp, axis=0))  # 0 列相加, 变成一行
    ret = np.where(lightList > 0)  # 返回（索引+ 数据类型 ）
    lightIdx = ret[0]
    rightShift = np.roll(lightIdx, -1)  # 向左移动一位
    differList = lightIdx - rightShift
    minValue = min(differList)
    temp = differList.tolist()
    colUp = lightIdx[temp.index(minValue)]
    # 计算中心点行坐标
    n = 0  # 存储该列有无光点
    sum_RowIdx = 0  # 该列的中心点行索引
    for row in range(40):
        if finRoiUp[row, colUp] != 0:
            n += 1
            sum_RowIdx += row + 1
    RowUp = round(sum_RowIdx / n) - 1
    realRow_Up = oriUp_row + RowUp  # 在原始图像中的行坐标
    realCol_Up = oriUp_col + colUp
    cv.circle(imgUp, (realCol_Up, realRow_Up), 1, (255, 0, 255), -1)

    # -----------对 下图最后窗口进行边缘点定位---------------
    finRoiDn_Red = finRoiDn[:, :, 2]  # B0 G1 R2
    _, finRoiDn = cv.threshold(finRoiDn_Red, 140, 255, cv.THRESH_BINARY)  # 转为二值图
    # 求最后的光条列坐标
    lightList = (np.sum(finRoiDn, axis=0))  # 0 列相加, 变成一行
    ret = np.where(lightList > 0)  # 返回（索引+ 数据类型 ）
    lightIdx = ret[0]
    rightShift = np.roll(lightIdx, -1)
    differList = lightIdx - rightShift
    minValue = min(differList)
    temp = differList.tolist()
    colDn = lightIdx[temp.index(minValue)]
    # 计算中心点行坐标
    n = 0  # 存储该列有无光点
    sum_RowIdx = 0  # 该列的中心点行索引
    for row in range(40):
        if finRoiDn[row, colDn] != 0:
            n += 1
            sum_RowIdx += row + 1
    RowDn = int(sum_RowIdx / n) - 1
    realRow_Dn = oriDn_row + RowDn  # 在原始图像中的行坐标
    realCol_Dn = oriDn_col + colDn
    cv.circle(imgDn, (realCol_Dn, realRow_Dn), 1, (255, 0, 255), -1)
    print("上边缘点列，行坐标：", (realCol_Up, realRow_Up))
    print("下边缘点列，行坐标：", (realCol_Dn, realRow_Dn))

    cv.imwrite(r"F:\MVS_Data\imgUp.png", imgUp)
    cv.imwrite(r"F:\MVS_Data\imgDn.png", imgDn)
    return (realCol_Up, realRow_Up), (realCol_Dn, realRow_Dn)


# 传入
def weldLine(img, P1, EP1, P2, EP2, crossP, P1_w=None, P2_w=None):
    print("***************** weldLine *****************")
    print("特征点坐标:", P1, P2)  # (x, y)
    print("边界点坐标:", EP1, EP2)  # (x, y)

    fp1X, fp1Y = P1  # laserL1  右下
    fp2X, fp2Y = P2  # laserL2  左下

    if fp1X < fp2X:
        print("焊缝在交叉点上方")
    else:
        print("焊缝在交叉点下方")

    """
    # singleBevel右光条由于照射方向的原因，会出现1个像素的泛光，这里是补偿误差
    if fp1X > fp2X:
        fp1Y = fp1Y - 1
    else:
        fp2Y = fp2Y - 1
    """
    bound1X, bound1Y = EP1
    bound2X, bound2Y = EP2

    fp_Lx, fp_Rx = min(fp1X, fp2X) - 100, max(bound1X, bound2X) + 100
    weld_k = (fp1Y - fp2Y) / (fp1X - fp2X)  # 斜率
    degree = math.atan(weld_k) / 3.1415926 * 180  # 角度
    # 焊缝特征线的直线方程参数
    A = weld_k
    B = -1
    C = fp1Y - weld_k * fp1X

    CornerUp = 90.2  # 90.5
    radUp = (degree + CornerUp) * 3.1415926 / 180
    print("radUpk:", radUp)
    boundUp_k = math.tan(radUp)  # 传入的是弧度
    print("boundUp_k:", boundUp_k)

    CornerDn = 90
    boundDn_k = math.tan(degree + (180 - CornerUp))

    if weld_k != 0:
        weld_b = fp1Y - weld_k * fp1X
        fp_Ly = weld_k * fp_Lx + weld_b
        fp_Ry = weld_k * fp_Rx + weld_b
        # 绘制焊缝线
        cv.line(img, (fp_Lx, round(fp_Ly)), (fp_Rx, round(fp_Ry)), (255, 255, 0), 1)  # 青蓝
        # 绘制垂线
        boundK = -1 / weld_k

        bound_b1 = bound1Y - boundK * bound1X
        init1X = round((weld_b - bound_b1) / (boundK - weld_k))
        init1Y = round(weld_k * init1X + weld_b)
        cv.line(img, (init1X, init1Y), (bound1X, bound1Y), (0, 255, 255), 1)

        bound_b2 = bound2Y - boundUp_k * bound2X
        init2X = round((weld_b - bound_b2) / (boundUp_k - weld_k))
        init2Y = round(weld_k * init2X + weld_b)
        cv.line(img, (init2X, init2Y), (bound2X, bound2Y), (0, 255, 255), 1)
    else:
        # 绘制焊缝线
        cv.line(img, (fp_Lx, round(fp1Y)), (fp_Rx, round(fp1Y)), (255, 255, 0), 1)  # 青蓝
        # 绘制垂线
        init1X, init2X = bound1X, bound2X
        init1Y, init2Y = fp1Y, fp1Y
        cv.line(img, (init1X, init1Y), (bound1X, bound1Y), (0, 255, 255), 1)
        cv.line(img, (init2X, init2Y), (bound2X, bound2Y), (0, 255, 255), 1)

    # 标记所有特征点
    # img[fp1Y, fp1X] = [255, 0, 255]
    cv.circle(img, [fp1X, fp1Y], 1, (255, 0, 255), -1)
    cv.circle(img, [fp2X, fp2Y], 1, (255, 0, 255), -1)
    cv.circle(img, [bound1X, bound1Y], 1, (255, 0, 255), -1)
    cv.circle(img, [bound2X, bound2Y], 1, (255, 0, 255), -1)

    cv.circle(img, [init1X, init1Y], 1, (255, 0, 255), -1)
    cv.circle(img, [init1X, init1Y], 5, (255, 0, 255), 1)

    cv.circle(img, [init2X, init2Y], 1, (255, 0, 255), -1)
    cv.circle(img, [init2X, init2Y], 5, (255, 0, 255), 1)

    # 禁止摆放区域
    cv.circle(img, crossP, 40, (0, 0, 255), 2)
    cv.circle(img, crossP, 2, (255, 0, 255), -1)
    ###########################################################################################
    print("-------------- 焊接参数（归纳）--------------")
    print(" 1. 初始点坐标 ")
    print("     初始点1坐标:", (init1Y, init1X))
    print("     初始点2坐标:", (init2Y, init2X))
    ###########################################################################################
    print(" 2. 坡口宽度 ")
    # 计算宽度
    if P1_w is not None:
        if isinstance(P1_w[1], int):  # 传入一个点
            x_P1w, y_P1w = P1_w
            x_P1, y_P1 = P1
            d1_1 = math.fabs(A * x_P1w + B * y_P1w + C) / math.sqrt(A ** 2 + B ** 2)
            d1_2 = math.fabs(A * x_P1 + B * y_P1 + C) / math.sqrt(A ** 2 + B ** 2)
            print("     laser1 坡口宽度:{:.2f}".format(d1_1 + d1_2))
            cv.circle(img, P1_w, 1, (255, 0, 255), -1)
            cv.circle(img, P1_w, 5, (255, 0, 255), 1)
        if isinstance(P1_w[1], (tuple, list)):  # 传入2个点
            x1_P1w, y1_P1w = P1_w[0]
            x2_P1w, y2_P1w = P1_w[1]
            d1_1 = math.fabs(A * x1_P1w + B * y1_P1w + C) / math.sqrt(A ** 2 + B ** 2)
            d1_2 = math.fabs(A * x2_P1w + B * y2_P1w + C) / math.sqrt(A ** 2 + B ** 2)
            print("     laser1 坡口宽度:{:.2f}".format(d1_1 + d1_2))
            cv.circle(img, P1_w[0], 1, (255, 0, 255), -1)
            cv.circle(img, P1_w[0], 5, (255, 0, 255), 1)
            cv.circle(img, P1_w[1], 1, (255, 0, 255), -1)
            cv.circle(img, P1_w[1], 5, (255, 0, 255), 1)
    else:
        print("     该类型无坡口宽度")
    if P2_w is not None:
        if isinstance(P2_w[1], int):  # 传入一个点
            x_P2w, y_P2w = P2_w  # （行，列）
            x_P2, y_P2 = P2
            d2_1 = math.fabs(A * x_P2w + B * y_P2w + C) / math.sqrt(A ** 2 + B ** 2)
            d2_2 = math.fabs(A * x_P2 + B * y_P2 + C) / math.sqrt(A ** 2 + B ** 2)
            print("     laser2 坡口宽度:{:.2f}".format(d2_1 + d2_2))
            cv.circle(img, P2_w, 1, (255, 0, 255), -1)
            cv.circle(img, P2_w, 5, (255, 0, 255), 1)
        if isinstance(P2_w[1], (tuple, list)):  # 传入2个点
            x1_P2w, y1_P2w = P2_w[0]
            x2_P2w, y2_P2w = P2_w[1]
            d2_1 = math.fabs(A * x1_P2w + B * y1_P2w + C) / math.sqrt(A ** 2 + B ** 2)
            d2_2 = math.fabs(A * x2_P2w + B * y2_P2w + C) / math.sqrt(A ** 2 + B ** 2)
            print("     laser2 坡口宽度:{:.2f}".format(d2_1 + d2_2))
            cv.circle(img, P2_w[0], 1, (255, 0, 255), -1)
            cv.circle(img, P2_w[0], 5, (255, 0, 255), 1)
            cv.circle(img, P2_w[1], 1, (255, 0, 255), -1)
            cv.circle(img, P2_w[1], 5, (255, 0, 255), 1)
    # else:
    #     print("     该类型无坡口宽度")
    ###########################################################################################
    print(" 3. 焊缝倾斜角度 ")
    print("     向下倾斜:{:.1f}°, 斜率:{:.3f},".format(degree, weld_k))
    ###########################################################################################
    print(" 4. 初始点错位距离 ")
    # cv.line(src, (initialP1_x, initialP1_y), (initialP2_x, initialP2_y), (0, 0, 255), 2)
    missDistance = math.sqrt((init1Y - init2Y) ** 2 + (init1X - init2X) ** 2)
    if missDistance > 20:
        print("     错位距离：%0.2f , 工件未对齐" % missDistance)
    else:
        print("     错位距离：%0.2f , 工件对齐" % missDistance)
    ###########################################################################################
    # cv.imshow("Results", img)
    cv.imwrite(r"F:\MVS_Data\Results.png", img)
